
/***********************************************************************
 *    Copyright(c) 2023-1112,    Co.,Ltd.
 *    PRODUCT  : lowpass
 *    FILENAME  : lowpass.c
 *
 *    PURPOSE  :  lowpass control
 *                       
 *    Encoding : GB2312                    
 *    HISTORY  :
 *           DATE                          VERSION                   AUTHOR                         NOTE
 *           202311-12                     V1.0                      xxx                          Created.
 *
 *  
 *  Software Development Evironment: 
 * -- 
 *  -- BIOS 
 * -- Code Generation 

 ************************************************************************/

#include "control.h"

#ifdef Lowpass_Control

#ifdef DSP28335RamRun
    #pragma CODE_SECTION(Lowpass2Ctrl, "ControlLoopInRAM")
#endif

// 双线性法
// void Init_Lowpass2ctrl(Lowpass2 * S , float32 w, float32 h)
// {
//     float32 c_a = w*h/(2 + w*h);  //  c/a
//     float32 b_a = (w*h - 2)/(w*h + 2);  //  b/a

//     S->Gx = c_a * c_a;
//     S->Gx1 = 2 * c_a * c_a;
//     S->Gx2 = S->Gx;
//     S->Gy1 = -2*b_a;
//     S->Gy2 = -b_a * b_a;

//     S->Xn = 0;   // x(n)
//     S->Xn1 = 0;   // x(n-1)
//     S->Xn2 = 0;   // x(n-2)
//     S->Yn = 0;   // y(n)
//     S->Yn1 = 0;   // y(n-1)
//     S->Yn2 = 0;   // y(n-1)
// }

// float32 Lowpass2Ctrl(Lowpass2 * S , float32 x)
// {
//     S->Xn2 = S->Xn1;
//     S->Xn1 = S->Xn;
//     S->Xn = x ;

//     S->Yn2 = S->Yn1;
//     S->Yn1 = S->Yn;
//     S->Yn = S->Gx * S->Xn + S->Gx1 * S->Xn1 + S->Gx2 * S->Xn2 + S->Gy1 * S->Yn1 + S->Gy2 * S->Yn2 ;

//     return S->Yn;
// }

// 近似差分 s 域 欧拉法
void Init_Lowpass2ctrl(Lowpass2 * S , float32 w, float32 h)
{
    float32 a = w*h+1;  

    S->w = w;
    S->Gx = (1-1/a)*(1-1/a);
    S->Gy1 = 2/a;
    S->Gy2 = -1/a/a;

    S->Xn = 0;   // x(n)
    S->Xn1 = 0;   // x(n-1)
    S->Xn2 = 0;   // x(n-2)
    S->Yn = 0;   // y(n)
    S->Yn1 = 0;   // y(n-1)
    S->Yn2 = 0;   // y(n-1)
}

float32 Lowpass2Ctrl(Lowpass2 * S , float32 x)
{
    S->Xn2 = S->Xn1;
    S->Xn1 = S->Xn;
    S->Xn = x ;

    S->Yn2 = S->Yn1;
    S->Yn1 = S->Yn;
    S->Yn = S->Gx * S->Xn + S->Gy1 * S->Yn1 + S->Gy2 * S->Yn2 ;
    
    return S->Yn;
}



void SetLowpass2(Lowpass2 * S , float32 w, float32 h)
{
    float32 a = w*h+1;  
    S->w = w;
    S->Gx = (1-1/a)*(1-1/a);
    S->Gy1 = 2/a;
    S->Gy2 = -1/a/a;
}

#endif

